#include "Collider.h"
#include "ICollidable.h"
#include <vector>
#include "Vertex.h"

using namespace std;

void Collider::CollideAll(){
	vector<ICollidable*>::iterator i, j;

	for (i = list->begin(); i != list->end(); i++)
		for (j = i + 1; j != list->end(); j++){
			Vertex v = (*j)->ClosestPoint((*i)->getCenter());		

			if ((*i)->InObject(v)){
				Vertex c1 = (*i)->getCenter(), c2 = (*j)->getCenter();
				Vertex v1 = (*i)->GetV(), v2 = (*j)->GetV();
				float m1 = (*i)->GetM(), m2 = (*j)->GetM(), I1 = (*i)->GetI(), I2 = (*j)->GetI();
				float v_angle1 = (*i)->GetV_angle(), v_angle2 = (*j)->GetV_angle();
				
				if ((*j)->IsEdgeCollision(v)){
					Vertex n = (*j)->GetNormal(v);
					float p = CalcImpulse(c1, c2, v, v1, v2, n, v_angle1, v_angle2, m1, m2, I1, I2);
					(*i)->collide(p, n, v, c1);
					(*j)->collide(-p, n, v, c2);
				}
				else if ((*i)->IsEdgeCollision(v)){
					Vertex n = (*i)->GetNormal(v);
					float p = CalcImpulse(c2, c1, v, v2, v1, n, v_angle2, v_angle1, m2, m1, I2, I1);
					(*i)->collide(-p, n, v, c1);
					(*j)->collide(p, n, v, c2);
				}
				
			}
		}

}

void Collider::update(double time){
	CollideAll();
}

float Collider::CalcImpulse(Vertex c1, Vertex c2, Vertex p, Vertex v1, Vertex v2, Vertex n, float v_angle1, float v_angle2, float m1, float m2, float I1, float I2){
	Vertex r1 = diff(c1, p);
	Vertex r2 = diff(c2, p);
	Vertex V;
	V.x = v1.x - v_angle1*r1.y - v2.x + v_angle2*r2.y;
	V.x = v1.y + v_angle1*r1.x - v2.y - v_angle2*r2.x;
	float e = 0.1, j;
	j = -(1 + e)*dot_product(V, n)/(1/m1 + 1/m2 + dot_product(r1, n)*dot_product(r1, n)/I1 + dot_product(r2, n)*dot_product(r2, n)/I2);
	return j;
}

Collider *Collider::instance = NULL;
std::vector<ICollidable*> *Collider::list = new vector<ICollidable*>();